#include "gripper_control_node.h"

GripperControlNode::GripperControlNode() : Node("gripper_control_node"),left_gripper(ctrl_port, 1),right_gripper(ctrl_port, 2)
{
    // 创建服务
    gripper_control_srv_ = this->create_service<gripper_control::srv::GripperControl>(
        "gripper_control", std::bind(&GripperControlNode::handle_gripper_request, this, std::placeholders::_1, std::placeholders::_2));
    // 创建发布者
    status_pub_ = this->create_publisher<gripper_control::msg::GripperStatus>("gripper_status", 10);
    // 创建定时器，每隔33ms（30Hz）发布一次状态
    timer_ = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&GripperControlNode::publish_gripper_status, this));
    RCLCPP_INFO(this->get_logger(), "Gripper service is ready.");
    
    // 设置目标位置、速度和扭矩
    // target.position = TARGET_POS_CLOSE; // 开合位置，默认关闭
    target.speed = TARGET_DEFAULT_SPEED;     // 默认速度
    target.torque = TARGET_DEFAULT_TORQUE;    // 默认扭矩

    // 初始状态夹爪均为闭合
    // left_gripper_is_open = false;
    // right_gripper_is_open = false;  
    init_gripper_pos();
}

void GripperControlNode::init_gripper_pos()
{
    left_gripper_is_open = false;
    right_gripper_is_open = false;  
    for(int i = 0; i < 3; i++)
    {
        target.position = TARGET_POS_OPEN;
        left_gripper.setTarget(target);
        left_gripper.control();

        status.position = 0;
        status.pos_reached = 0;
        left_gripper.updateStatus();
        left_gripper.getStatus(&status);

        if(status.pos_reached == 0)
        {
            left_gripper_is_open = true;
            break;
        }
        else if((status.pos_reached == 1) && (status.position < 2000))
        {
            left_gripper_is_open = true;
            break;
        }
    }
    if(left_gripper_is_open)
    {
        std::cout << "Left gripper pos is inited." << std::endl;
    }
    else 
    {
        std::cout << "Left gripper pos initialization failed." << std::endl;
    }

    for(int i = 0; i < 3; i++)
    {
        target.position = TARGET_POS_OPEN;
        right_gripper.setTarget(target);
        right_gripper.control();

        status.position = 0;
        status.pos_reached = 0;
        right_gripper.updateStatus();
        right_gripper.getStatus(&status);
        
        if(status.pos_reached == 0)
        {
            right_gripper_is_open = true;
            break;
        }
        else if((status.pos_reached == 1) && (status.position < 2000))
        {
            right_gripper_is_open = true;
            break;
        }
    }
    if(right_gripper_is_open)
    {
        std::cout << "Right gripper pos is inited." << std::endl;
    }
    else 
    {
        std::cout << "Right gripper pos initialization failed." << std::endl;
    }
}

void GripperControlNode::handle_gripper_request(
    const std::shared_ptr<gripper_control::srv::GripperControl::Request> request,
    const std::shared_ptr<gripper_control::srv::GripperControl::Response> response)
{
     bool left_command = request->gripper_control[0];  // 左夹爪控制命令
     bool right_command = request->gripper_control[1]; // 右夹爪控制命令
    // 根据请求控制夹爪
    // 左夹爪
    if(left_command && (!left_gripper_is_open)) 
    {
        std::cout << "Opening left gripper..." << std::endl;
        
        target.position = TARGET_POS_OPEN;
        left_gripper.setTarget(target);
        left_gripper.control();

        status.position = 0;
        status.pos_reached = 0;
        left_gripper.updateStatus();
        left_gripper.getStatus(&status);

        if(status.pos_reached == 0)
        {
            left_gripper_is_open = true;
        }
        else if((status.pos_reached == 1) && (status.position < 2000))
        {
            left_gripper_is_open = true;
        }
        else if((status.pos_reached == 1) && (status.position > 8000))
        {
            left_gripper_is_open = false;
        }
    } 
    else if((!left_command) && left_gripper_is_open)
    {
        std::cout << "Closing left gripper..." << std::endl;
        
        target.position = TARGET_POS_CLOSE;
        left_gripper.setTarget(target);
        left_gripper.control();

        status.position = 0;
        status.pos_reached = 0;
        left_gripper.updateStatus();
        left_gripper.getStatus(&status);
        
        if(status.pos_reached == 0)
        {
            left_gripper_is_open = false;
        }
        else if((status.pos_reached == 1) && (status.position < 2000))
        {
            left_gripper_is_open = true;
        }
        else if((status.pos_reached == 1) && (status.position > 8000))
        {
            left_gripper_is_open = false;
        }
    }
    // 右夹爪
    if(right_command && (!right_gripper_is_open)) 
    {
        std::cout << "Opening right gripper..." << std::endl;

        target.position = TARGET_POS_OPEN;
        right_gripper.setTarget(target);
        right_gripper.control();

        status.position = 0;
        status.pos_reached = 0;
        right_gripper.updateStatus();
        right_gripper.getStatus(&status);
        
        if(status.pos_reached == 0)
        {
            right_gripper_is_open = true;
        }
        else if((status.pos_reached == 1) && (status.position < 2000))
        {
            right_gripper_is_open = true;
        }
        else if((status.pos_reached == 1) && (status.position > 8000))
        {
            right_gripper_is_open = false;
        }
    } 
    else if((!right_command) && right_gripper_is_open)
    {
        std::cout << "Closing right gripper..." << std::endl;
        
        target.position = TARGET_POS_CLOSE;
        right_gripper.setTarget(target);
        right_gripper.control();

        status.position = 0;
        status.pos_reached = 0;
        right_gripper.updateStatus();
        right_gripper.getStatus(&status);

        if(status.pos_reached == 0)
        {
            right_gripper_is_open = false;
        }
        else if((status.pos_reached == 1) && (status.position < 2000))
        {
            right_gripper_is_open = true;
        }
        else if((status.pos_reached == 1) && (status.position > 8000))
        {
            right_gripper_is_open = false;
        }
    }
    response->gripper_status[0] = left_gripper_is_open;
    response->gripper_status[1] = right_gripper_is_open;
    RCLCPP_INFO(this->get_logger(), 
                    "Executed gripper control: Left=%s, Right=%s",
                    left_command ? "OPEN" : "CLOSED",
                    right_command ? "OPEN" : "CLOSED");
}

void GripperControlNode::publish_gripper_status()
{        
        // 发布消息
        auto msg = gripper_control::msg::GripperStatus();
        msg.gripper[0] = left_gripper_is_open;   // 左夹爪
        msg.gripper[1] = right_gripper_is_open;  // 右夹爪
        
        status_pub_->publish(msg);
}